#ifndef MATRIX_4_H_
#define MATRIX_4_H_

#include "Vec4.h"
#include "Matrix3.h"
#include "SDL_OpenGL.h"
/*Collum major Matrix 4 class 
************************
 * [0]  [4]  [8]  [12] *
 * [1]  [5]  [9]  [13] *
 * [2]  [6]  [10] [14] *
 * [3]  [7]  [11] [15] *
 ***********************/
class Matrix4
{
public:
        /* Public data */
        float m_data[16];
public:
    /* Deafualt ctr creats an identity matrix */
    Matrix4();
    /* Overloaded Ctr */
    Matrix4(const float* d);
    /* Sets the major diagonals to 1 */
    void LoadIdentity();
    /* Set the Major Diagonals */
    Matrix4& SetDiagonals( float a, float b, float c);
    /* Overloaded Operators */
    Matrix4 operator*(const Matrix4& other) const;
    Matrix4& operator=(const Matrix4& other);
    Matrix4& operator*=(const Matrix4& other);
    Vec3 operator*(const Vec3& v) const;
    Vec4 operator*(const Vec4& v) const;

    /* Transform the given vector by this matrix */
    Vec3 Transform(const Vec3& v) const;
    Vec3 TransformInverse(const Vec3& point) const;
    /* Transforms a direction vector by this matrix */
    Vec3 TransformDirection(const Vec3& vector) const;
    Vec3 TransformInverseDirection(const Vec3& v) const;

    /* Get the determinant of this matrix */
    float GetDeterminant() const;
    /* Sets this matrix to be the inverse of the given matrix */
    void SetInverse(const Matrix4& m);
    /* Gets a copy of the inverted version of this matrix */
    Matrix4 Inverse() const;
    /* Inverts this matrix */
    Matrix4& Invert();

    /* Sets the Transaltion components of this matrix */
    Matrix4& SetTranslate(const Vec3& pos);
    Matrix4& SetTranslate(const float x, const float y, const float z);

    /* Simplified set functions */
    Matrix4& SetRotationX(float angle);
    Matrix4& SetRotationY(float angle);
    Matrix4& SetRotationZ(float angle);
    Matrix4& SetScale(const Vec3& s);
    Matrix4& SetScale(const float x, const float y, const float z);

    /* Index 3 is the position of the matrix, */
    Vec3 GetAxisVecor(int index) const;
    void SetAxisVector(int index, const Vec3& vec);
    /* Index 0 is data 0,4,8 */
    Vec3 GetRowVector(int index) const;
    /* Sets this matrix to be the rotation matrix according to 
     * the given quaternion */
    Matrix4& SetOrientationAndPosition(const Quaternion& q, const Vec3& pos);
    Matrix4& SetOrientation(const Quaternion& q);
    void FillOpenGlArray(float* arr) const;

    /* Fer Debugin' */
    void Print() const;
};

namespace Matrix
{
    /* Loads the necessary OpenGL Matrix */
    static Matrix4 GetModelViewMatrix()
    {
        float m[16];
        glGetFloatv(GL_MODELVIEW_MATRIX, m);
        return Matrix4(m);
    }
    static Matrix4 GetProjectionMatrix()
    {
        float m[16];
        glGetFloatv(GL_PROJECTION_MATRIX, m);
        return Matrix4(m);
    } 

    /* Multiply the OpenGL matrix by *this */
    static void Multiply(const Matrix4& m)
    {
        glMultMatrixf(m.m_data);
    }

    /* Overly fucking complicated function that sets the matrix parameters stats
     * according to the position and quaternion passed in.
     
     * Update: Makes more sense now .

      Update: ToDO:  CHANGE THIS: use improved version frmo Matrix4 class */
    static inline void CalculateTransformMatrix(Matrix4& m, 
                                                const Vec3& position, 
                                                const Quaternion& orientation)
    {
        m.m_data[0] = 1 - 2 * orientation.m_j * orientation.m_j-
                           2 * orientation.m_k * orientation.m_k;
        m.m_data[4] =      2 * orientation.m_i * orientation.m_j-
                           2 * orientation.m_r * orientation.m_k;
        m.m_data[8] =     2 * orientation.m_i * orientation.m_k+
                           2 * orientation.m_r * orientation.m_j;
        m.m_data[12] = position.X();

        m.m_data[1] =     2 * orientation.m_i * orientation.m_j+
                           2 * orientation.m_r * orientation.m_k;
        m.m_data[5] = 1 - 2 * orientation.m_i * orientation.m_i-
                           2 * orientation.m_k * orientation.m_k;
        m.m_data[9] =     2 * orientation.m_j * orientation.m_k-
                           2 * orientation.m_r * orientation.m_i;
        m.m_data[13] = position.Y();

        m.m_data[2] =      2 * orientation.m_i * orientation.m_k-
                            2 * orientation.m_r * orientation.m_j;
        m.m_data[6] =      2 * orientation.m_j * orientation.m_k+
                            2 * orientation.m_r * orientation.m_i;
        m.m_data[10] = 1 - 2 * orientation.m_i * orientation.m_i-
                            2 * orientation.m_j * orientation.m_j;
        m.m_data[14] = position.Z();
    }
    /*************************************************************************
     * Internal function to do an intertia tensor transform by a quaternion. *
     * Note that the implementation of this function was created by an       *
     * automated code-generator and optimizer.                               *
     *************************************************************************/

    /* Update: This still makes no sense. Use local inertia Tensor to understand
     * as it is more straight forward....*/
    static inline void TransformInertiaTensor(  Matrix3& iitWorld,
                                                const Quaternion& q,
                                                const Matrix3& iitBody,
                                                const Matrix4& rotmat)
    {
        float t4 =  rotmat.m_data[0]*iitBody.m_data[0]+
                    rotmat.m_data[4]*iitBody.m_data[1]+
                    rotmat.m_data[8]*iitBody.m_data[2];
        float t9 =  rotmat.m_data[0]*iitBody.m_data[3]+
                    rotmat.m_data[4]*iitBody.m_data[4]+
                    rotmat.m_data[8]*iitBody.m_data[5];
        float t14 = rotmat.m_data[0]*iitBody.m_data[6]+
                    rotmat.m_data[4]*iitBody.m_data[7]+
                    rotmat.m_data[8]*iitBody.m_data[8];
        float t28 = rotmat.m_data[1]*iitBody.m_data[0]+
                    rotmat.m_data[5]*iitBody.m_data[1]+
                    rotmat.m_data[9]*iitBody.m_data[2];
        float t33 = rotmat.m_data[1]*iitBody.m_data[3]+
                    rotmat.m_data[5]*iitBody.m_data[4]+
                    rotmat.m_data[9]*iitBody.m_data[5];
        float t38 = rotmat.m_data[1]*iitBody.m_data[6]+
                    rotmat.m_data[5]*iitBody.m_data[7]+
                    rotmat.m_data[6]*iitBody.m_data[8];
        float t52 = rotmat.m_data[8]*iitBody.m_data[0]+
                    rotmat.m_data[6]*iitBody.m_data[1]+
                    rotmat.m_data[10]*iitBody.m_data[2];
        float t57 = rotmat.m_data[2]*iitBody.m_data[3]+
                    rotmat.m_data[6]*iitBody.m_data[4]+
                    rotmat.m_data[10]*iitBody.m_data[5];
        float t62 = rotmat.m_data[2]*iitBody.m_data[6]+
                    rotmat.m_data[6]*iitBody.m_data[7]+
                    rotmat.m_data[10]*iitBody.m_data[8];

        iitWorld.m_data[0] =    t4*rotmat.m_data[0]+
                                t9*rotmat.m_data[4]+
                                t14*rotmat.m_data[8];
        iitWorld.m_data[3] =    t4*rotmat.m_data[1]+
                                t9*rotmat.m_data[5]+
                                t14*rotmat.m_data[9];
        iitWorld.m_data[6] =    t4*rotmat.m_data[2]+
                                t9*rotmat.m_data[6]+
                                t14*rotmat.m_data[10];
        iitWorld.m_data[1] =    t28*rotmat.m_data[0]+
                                t33*rotmat.m_data[4]+
                                t38*rotmat.m_data[8];
        iitWorld.m_data[4] =    t28*rotmat.m_data[1]+
                                t33*rotmat.m_data[5]+
                                t38*rotmat.m_data[9];
        iitWorld.m_data[7] =    t28*rotmat.m_data[2]+
                                t33*rotmat.m_data[6]+
                                t38*rotmat.m_data[10];
        iitWorld.m_data[2] =    t52*rotmat.m_data[0]+
                                t57*rotmat.m_data[4]+
                                t62*rotmat.m_data[8];
        iitWorld.m_data[5] =    t52*rotmat.m_data[1]+
                                t57*rotmat.m_data[5]+
                                t62*rotmat.m_data[9];
        iitWorld.m_data[8] =    t52*rotmat.m_data[2]+
                                t57*rotmat.m_data[6]+
                                t62*rotmat.m_data[10];
    }

}//namespace

#endif